#include "cg_interfaces/BridgeControl.h"
#include "cg_interfaces/BridgeSensor.h"
#include "cg_interfaces/CanFrame.h"
#include <functional>
#include <ros/ros.h>

#include "encoder.hpp"

cg_interfaces::BridgeControl control_msg;
cg_interfaces::BridgeSensor sensor_msg;
unsigned char heart_steer = 0;
unsigned char heart_brake = 0;
MotorControlCfg motor_ctrl_cfg;

bool TryGetParams(int &id)
{
    ros::NodeHandle nh("~");
    nh.param("id", id, -1);
    if (id == -1)
    {
        if (!ros::NodeHandle().getParam("self", id))
        {
            ROS_ERROR("Failed to get id parameter.");
            return false;
        }
    }
    return true;
}

void OnReceiveControl(const cg_interfaces::BridgeControl::ConstPtr &msg)
{
    control_msg = *msg;
}

void OnPublishSteeringFrame(const ros::TimerEvent &event, const ros::Publisher &can_pub)
{
    double steering_angle = control_msg.steer_angle;
    cg_interfaces::CanFrame can_frame;
    can_frame.id = 0x118;
    can_frame.chanel = 1;
    EncodeSteeringFrame(can_frame.data.data(), steering_angle, heart_steer);
    can_pub.publish(can_frame);
}

void OnPublishBrakeFrame(const ros::TimerEvent &event, const ros::Publisher &can_pub)
{
    double brake = control_msg.brake_force;
    cg_interfaces::CanFrame can_frame;
    can_frame.id = 0x201;
    can_frame.chanel = 1;
    EncodeBrakeFrame(can_frame.data.data(), brake, heart_brake);
    can_pub.publish(can_frame);
}

void OnPublishMotorFrame(const ros::TimerEvent &event, const ros::Publisher &can_pub)
{
    double left = control_msg.torque_left;
    double right = control_msg.torque_right;
    cg_interfaces::CanFrame can_frame;
    can_frame.id = 0x011;
    can_frame.chanel = 1;
    //motor_ctrl_cfg.m1_value = left;
    //motor_ctrl_cfg.m2_value = right;
    motor_ctrl_cfg.Encode2MotorFrame(can_frame.data.data());
    can_pub.publish(can_frame);
}

void OnReceiveCanFrame(const cg_interfaces::CanFrame::ConstPtr &msg)
{
    // decode can frame
    switch (msg->id)
    {
    case 0x010:
        DecodeSpeed(reinterpret_cast<const unsigned char *>(msg->data.data()), sensor_msg.rpm_left);
        break;
    case 0x030:
        DecodeSpeed(reinterpret_cast<const unsigned char *>(msg->data.data()), sensor_msg.rpm_right);
        break;
    default:
        ROS_INFO("Receive Ignore CAN frame with id %d.", msg->id);
    }
}

int main(int argc, char **argv)
{
    const int depth = 5;
    ros::init(argc, argv, "local_bridge");
    ros::NodeHandle nh;
    int id;
    if (!TryGetParams(id))
    {
        ROS_ERROR("Failed to get id parameter.");
        return -1;
    }
    ROS_INFO("Local bridge with id %d started.", id);
    
    motor_ctrl_cfg.Init();
    
    motor_ctrl_cfg.m1_enable = false;
    motor_ctrl_cfg.m1_mode = MotorControlCfg::VelocityControl;
    motor_ctrl_cfg.m1_rotation = true;
    motor_ctrl_cfg.m1_value = 0;
    motor_ctrl_cfg.m1_speed_limit = 7665;
    motor_ctrl_cfg.m2_enable = true;
    motor_ctrl_cfg.m2_mode = MotorControlCfg::TorqueControl;
    motor_ctrl_cfg.m2_rotation = false;
    motor_ctrl_cfg.m2_value = 2000;
    motor_ctrl_cfg.m2_speed_limit = 0;
    motor_ctrl_cfg.power_limit =1000;
    motor_ctrl_cfg.hb =2;
    

    std::string topic_root = "bridge" + std::to_string(id) + '/';
    ros::Publisher can_pub = nh.advertise<cg_interfaces::CanFrame>("can_download", depth);
    ros::Subscriber can_sub = nh.subscribe<cg_interfaces::CanFrame>("can_upload", depth, OnReceiveCanFrame);
    ros::Publisher sensor_pub = nh.advertise<cg_interfaces::BridgeSensor>(topic_root + "sensor", depth);
    ros::Subscriber control_sub =
        nh.subscribe<cg_interfaces::BridgeControl>(topic_root + "control", depth, OnReceiveControl);
    ros::Timer timer_steering =
        nh.createTimer(ros::Duration(1.0 / 20), std::bind(OnPublishSteeringFrame, std::placeholders::_1, can_pub));
    ros::Timer timer_brake =
        nh.createTimer(ros::Duration(1.0 / 20), std::bind(OnPublishBrakeFrame, std::placeholders::_1, can_pub));
    ros::Timer timer_motor =
        nh.createTimer(ros::Duration(1.0 / 20), std::bind(OnPublishMotorFrame, std::placeholders::_1, can_pub));

    ros::spin();
    return 0;
}